#include "common_tool/csv_parser.h"
#include "common_tool/csv_writer.h"
#include "gen_circle_csv_config.pb.h"
#include "common_tool/proto_file.h"
#include "rtk_data_path.h"
#include "string"
#include "cmath"
int main(int argc, char **argv)
{
	/*rtk_planning::RepeatedCircleConfig rep_circle_config;
    rtk_planning::CircleConfig *circle_config_ptr;
    circle_config_ptr=rep_circle_config.add_circle_config();
    circle_config_ptr->set_radius(50);
    circle_config_ptr->set_x_center(0);
    circle_config_ptr->set_y_center(0);
    circle_config_ptr->set_v(10);

    circle_config_ptr=rep_circle_config.add_circle_config();
    circle_config_ptr->set_radius(100);
    circle_config_ptr->set_x_center(0);
    circle_config_ptr->set_y_center(0);
    circle_config_ptr->set_v(10);
    
    common_tool::SetProtoToASCIIFile(rep_circle_config,"repeated_circle_config.proto");*/
	rtk_planning::RepeatedCircleConfig rep_circle_config;
	common_tool::GetProtoFromASCIIFile(rtk_config_path + "repeated_circle_config.proto", &rep_circle_config);
	int size = rep_circle_config.circle_config_size();
	std::vector<std::string> head_string = {"x", "y", "theta", "kappa", "v", "s", "a"};
	std::string filename_prefix = "circle";
	for (int i = 0; i < size; i++)
	{
		double radius = rep_circle_config.circle_config(i).radius();
		double x_center = rep_circle_config.circle_config(i).x_center();
		double y_center = rep_circle_config.circle_config(i).y_center();
		double v = rep_circle_config.circle_config(i).v();
		double delta_theta = rep_circle_config.circle_config(i).delta_theta();
		std::cout << radius << " " << x_center << " " << y_center << " " << v << " " << delta_theta << std::endl;
		std::string filename = filename_prefix + "R" + std::to_string(int(radius)) + "X" + std::to_string(int(x_center)) + "Y" +
							   std::to_string(int(y_center)) + ".csv";
		io::CSVWriter rtk_csv(filename);
		rtk_csv.write_row<std::string>(head_string);
		for (int i = 0; i * delta_theta < 2 * 3.14159265; i++)
		{
			double x = radius * std::cos(i * delta_theta) + x_center;
			double y = radius * std::sin(i * delta_theta) + y_center;
			double theta = (3.14159265 / 2 + i * delta_theta) * 180 / 3.14159265;
			double s = i * delta_theta * radius;

			std::vector<double> temp_vector;
			temp_vector.push_back(x);
			temp_vector.push_back(y);
			temp_vector.push_back(theta);
			temp_vector.push_back(1 / radius);
			temp_vector.push_back(v * 3.6);
			temp_vector.push_back(s);
			temp_vector.push_back(0);
			rtk_csv.write_row<double>(temp_vector);
		}
	}
}